%%% Gait detection data analysis %%%
% ver 12/13/08
close all;
clear all;
clc;

%% Defining Constants
filename = 'no_orthosis_4_walk1';%'Block_2_4KM16';
% enter filename 'Block_X_YKMZ' as a filename for othosis case, enter
% 'no_orthosis_Y_walkZ' for no orthosis case
plate_length = 1.00; %m
plate_width = 0.30; %m
plate_offset = 0.30; %m
force_scale = 0.03; % scaling GRFs for visualization
force_filter = 8; %Hz
emg_filter_1 = 120; %Hz
emg_filter_2 = 10; %Hz
MA_filter = 8;%Hz
LKnee_offset = 0.07;
foot_width = 0.09;
ankle_size = 0.04;
knee_size = 0.05;
hip_size = 0.05;
leg_radius = 0.04;
thigh_radius = 0.05;
pelvis_height = 0.08;
foot_height = 0.04;
foot_scale = 2;

%% loading files
% load .anc file
[labels_anc,time_anc,EMG,rel_angle,forces] = readColDataANC(strcat(filename,'.anc'),22,11,1);

% load .trc file
[labels,time_trc,LASIS,RASIS,Sacrum,Sac_offset,LThigh_up,LThigh_front,LThigh_rear,...
    LShank_up, LShank_front, LShank_rear, LAnkle_lat,  LHeel, LToe, RThigh_up, RThigh_front,RThigh_rear,...  %switched RAnkle_lat and LAnkle_lat for no-orthosis_2km1
    RShank_up, RShank_front, RShank_rear, RAnkle_lat, RHeel, RToe]...
    = readColDataMA_dyn(strcat(filename,'.trc'),68,6,1);
% [time_trc,Rtoe,Rankle,Rheel,Ltoe,Lankle,Lheel] = readColDataXLS_MA(filename);

%% analyzing data
dt_trc = time_trc(2)-time_trc(1);
dt_anc = time_anc(2)-time_anc(1);
[b,a] = butter(4,8/500); forces = filtfilt(b,a,forces);
Lforce = forces(:,13:16)./204.8;
Lforce_totalz = sum(Lforce,2)+.00001;
Rforce = forces(:,5:8)./204.8;
Rforce_totalz = sum(Rforce,2)+.00001;

Lforce_x = forces(:,9:10)./204.8;
Lforce_totalx = sum(Lforce_x,2);
Lforce_y = forces(:,11:12)./204.8;
Lforce_totaly = sum(Lforce_y,2);
Lforce_mag = sqrt(Lforce_totalx.^2 + Lforce_totaly.^2 + Lforce_totalz.^2);
[b,a] = butter(4,force_filter/500); Lforce_mag = filtfilt(b,a,Lforce_mag);
Lforce_dir = [Lforce_totalx./Lforce_mag Lforce_totaly./Lforce_mag Lforce_totalz./Lforce_mag];
[b,a] = butter(4,force_filter/500); Lforce_dir = filtfilt(b,a,Lforce_dir);

Rforce_x = forces(:,1:2)./204.8;
Rforce_totalx = sum(Lforce_x,2);
Rforce_y = forces(:,3:4)./204.8;
Rforce_totaly = sum(Lforce_y,2);
Rforce_mag = sqrt(Rforce_totalx.^2 + Rforce_totaly.^2 + Rforce_totalz.^2);
[b,a] = butter(4,force_filter/500); Rforce_mag = filtfilt(b,a,Rforce_mag);
Rforce_dir = [Rforce_totalx./Rforce_mag Rforce_totaly./Rforce_mag Rforce_totalz./Rforce_mag];
[b,a] = butter(4,force_filter/500); Rforce_dir = filtfilt(b,a,Rforce_dir);
Rforce_mag = force_scale*Rforce_mag; Lforce_mag = force_scale*Lforce_mag;

CoP_L = [(Lforce(:,1)+Lforce(:,2))./Lforce_totalz * plate_length  (Lforce(:,1)+Lforce(:,4))./Lforce_totalz * plate_width + plate_offset];
CoP_R = [(Rforce(:,1)+Rforce(:,2))./Rforce_totalz * plate_length  (Rforce(:,1)+Rforce(:,4))./Rforce_totalz * plate_width];
[b,a] = butter(4,force_filter/500); CoP_L = filtfilt(b,a,CoP_L);
[b,a] = butter(4,force_filter/500); CoP_R = filtfilt(b,a,CoP_R);

torque = -rel_angle/204.8/.75*20*.308;

%% EMG
RF = EMG(:,1); VL = EMG(:,2); MH = EMG(:,3); AL = EMG(:,4);
RF = abs(RF - mean(RF));VL = abs(VL - mean(VL));MH = abs(MH - mean(MH));AL = abs(AL - mean(AL));
[b,a] = butter(8,emg_filter_1/500); RF = filtfilt(b,a,RF); [b,a] = butter(3,emg_filter_2/500); RF = filtfilt(b,a,RF);
[b,a] = butter(8,emg_filter_1/500); VL = filtfilt(b,a,VL); [b,a] = butter(3,emg_filter_2/500); VL = filtfilt(b,a,VL);
[b,a] = butter(8,emg_filter_1/500); MH = filtfilt(b,a,MH); [b,a] = butter(3,emg_filter_2/500); MH = filtfilt(b,a,MH);
[b,a] = butter(8,emg_filter_1/500); AL = filtfilt(b,a,AL); [b,a] = butter(3,emg_filter_2/500); AL = filtfilt(b,a,AL);

%%
%x-direction is in line with  treadmill
max_xdata = [RAnkle_lat(:,1) LAnkle_lat(:,1) RToe(:,1) LToe(:,1)]/1000;
max_ydata = [RToe(:,2) LToe(:,2) RHeel(:,2) LHeel(:,2)]/1000;
max_zdata = [zeros(length(Sacrum),1) Sacrum(:,3) Sac_offset(:,3)]/1000;

%% Parsing motion data
% LASIS,RASIS,Sacrum,Sac_offset,LThigh_up,LThigh_front,LThigh_rear,LKnee_lat,LKnee_med,...
%     LShank_up, LShank_front, LShank_rear, LAnkle_lat,LAnkle_med, LHeel, LToe, RThigh_up,RThigh_front,RThigh_rear,RKnee_lat,RKnee_med,...
%     RShank_up, RShank_front, RShank_rear, RAnkle_lat,RAnkle_med, RHeel, RToe, RTrochanter, LTrochanter]
xdata = [LASIS(:,1),RASIS(:,1),Sacrum(:,1),LThigh_up(:,1),LThigh_front(:,1),LThigh_rear(:,1),...
    LShank_up(:,1), LShank_front(:,1), LShank_rear(:,1), LAnkle_lat(:,1), LHeel(:,1),  LToe(:,1), RThigh_up(:,1),RThigh_front(:,1),RThigh_rear(:,1),...
    RShank_up(:,1), RShank_front(:,1), RShank_rear(:,1), RAnkle_lat(:,1), RHeel(:,1),  RToe(:,1), Sac_offset(:,1)]/1000;

ydata = [LASIS(:,2),RASIS(:,2),Sacrum(:,2),LThigh_up(:,2),LThigh_front(:,2),LThigh_rear(:,2),...
    LShank_up(:,2), LShank_front(:,2), LShank_rear(:,2), LAnkle_lat(:,2), LHeel(:,2), LToe(:,2), RThigh_up(:,2),RThigh_front(:,2),RThigh_rear(:,2),...
    RShank_up(:,2), RShank_front(:,2), RShank_rear(:,2), RAnkle_lat(:,2),RHeel(:,2), RToe(:,2), Sac_offset(:,2)]/1000;

zdata = [LASIS(:,3),RASIS(:,3),Sacrum(:,3),LThigh_up(:,3),LThigh_front(:,3),LThigh_rear(:,3),...
    LShank_up(:,3), LShank_front(:,3), LShank_rear(:,3), LAnkle_lat(:,3), LHeel(:,3), LToe(:,3), RThigh_up(:,3),RThigh_front(:,3),RThigh_rear(:,3),...
    RShank_up(:,3), RShank_front(:,3), RShank_rear(:,3), RAnkle_lat(:,3), RHeel(:,3), RToe(:,3), Sac_offset(:,3)]/1000;
[b,a] = butter(5,MA_filter/50); %100 Hz sampling rate
xdata = filtfilt(b,a,xdata); ydata = filtfilt(b,a,ydata); zdata = filtfilt(b,a,zdata);

%% Finding Heel Strike/Toe off
[LHS,LHS_locs] = findpeaks(xdata(:,12)); [RHS,RHS_locs] = findpeaks(xdata(:,21)); RHS_locs = [1 RHS_locs];
[LTO,LTO_locs] = findpeaks(-xdata(:,12)); [RTO,RTO_locs] = findpeaks(-xdata(:,21));
[Max_LToe Max_LToe_ind] = max(zdata(:,12)); [Max_RToe Max_RToe_ind] = max(zdata(:,21)); 
% Rankle_angle = atan2(RToe(:,3)-RHeel(:,3),RToe(:,1)-RHeel(:,1));

%% Kinematics


rthigh_angle = atan2(zdata(:,14)-zdata(:,15),xdata(:,14)-xdata(:,15))*180/pi ;
rshank_angle = atan2(zdata(:,17)-zdata(:,18),xdata(:,17)-xdata(:,18))*180/pi ;
rknee_angle = rthigh_angle-rshank_angle;
rknee_vel = diff(rknee_angle)/dt_trc;
[b,a] = butter(5,MA_filter/50); %100 Hz sampling rate
rknee_vel = filtfilt(b,a,rknee_vel);

lthigh_angle = atan2(zdata(:,5)-zdata(:,6),xdata(:,5)-xdata(:,6))*180/pi ;
lshank_angle = atan2(zdata(:,8)-zdata(:,9),xdata(:,8)-xdata(:,9))*180/pi ;
lknee_angle = lthigh_angle-lshank_angle;
lknee_vel = diff(lknee_angle)/dt_trc;
[b,a] = butter(5,MA_filter/50); %100 Hz sampling rate
lknee_vel = filtfilt(b,a,lknee_vel);

RKnee_0 = [566.716610000000, -426.814480000000, 542.484860000000]/1000;
LKnee_0 = [84.3858600000000,577.924440000000,544.943790000000];

CoR_matrix = ...  % First line is for othosis case, second line is for no orthosis case
...% [-0.0398582167379422,-0.176006866105204,-0.0104850655131230,1;0.111925607277528,-0.0740089849060460,0.0197952669446540,1;0.0538494816049560,0.180916058465820,0.0757569229476501,1;-0.222732901289444,0.0740663770462788,-0.00442546642409980,1;-0.00569337793172464,0.223229858400418,-0.160324032338959,1;-0.240247234957734,0.0718021320039221,0.118176011348530,1;0.207530096269587,-0.142799949281605,0.0207130311651393,1;0.0732496720996777,-0.0644934978645009,0.0327540360967806,1;0.159019372309612,-0.0792541497162440,-0.213121671679041,1;0.0711304027748942,-0.0635989310717027,0.156599542633235,1;-0.0233792398271668,-0.00505636677548643,0.0793077050863173,1;-0.0177830326442321,-0.00227800969466818,-0.0818352991319228,1;];
[0.165835838530992,-0.103770700827843,-0.0426762975397763,1;0.154354492366010,-0.0687180287726588,0.0223663219229540,1;-0.160195652329756,0.147213206660916,0.0247296714385204,1;-0.207705363232106,0.0639846689162971,-0.00183380715445058,1;-0.183690039211880,0.169649830970690,-0.0881131179772962,1;-0.244483631102654,0.0761708899721617,0.117323356527970,1;0.153044731536819,-0.0984182955837206,-0.0498976686186917,1;0.0892617497222636,-0.0720472020750166,0.0309807728846054,1;0.127598204332748,-0.0951204270189359,-0.164485982877647,1;0.0691317287861592,-0.0573634125231667,0.153776307044152,1;-0.0177573943120506,0.00246057978393044,0.0831400990022171,1;-0.0143148989412387,-0.00902411393148239,-0.0820420201923487,1;];
LHip_1 = CoR_matrix(1,:);
RHip_1 = CoR_matrix(2,:);
LKnee_1 = CoR_matrix(3,:);
RKnee_1 = CoR_matrix(4,:);
LKnee_in_1 = CoR_matrix(5,:);
RKnee_in_1 = CoR_matrix(6,:);
LKnee_2 = CoR_matrix(7,:);
RKnee_2 = CoR_matrix(8,:);
LKnee_in_2 = CoR_matrix(9,:);
RKnee_in_2 = CoR_matrix(10,:);
Lank = CoR_matrix(11,:);
Rank = CoR_matrix(12,:);

%% Extra Body Coords
l_firstmet = [xdata(:,12) ydata(:,12)-foot_width zdata(:,12)];
l_midmet = [xdata(:,12) ydata(:,12)-foot_width/2 zdata(:,12)];
r_firstmet = [xdata(:,21) ydata(:,21)+foot_width zdata(:,21)];
r_midmet = [xdata(:,21) ydata(:,21)+foot_width/2 zdata(:,21)];

%% plotting

fh3 = figure(3);
hold on;
axis([min(min(max_xdata)) max(max(max_xdata)) min(min(max_ydata)) max(max(max_ydata)) min(min(max_zdata)) max(max(max_zdata))]);
% axis([ 0 2 0 2 0 1.5]);
view(180,0);
 axis equal;
 axis([ 0 1.2 0 1 0 1.1]);
xlabel('Xdir')
ylabel('Ydir')

ColorVec = [hsv2rgb([.5 .0 .5]);hsv2rgb([1 .0 .2]);];


% PELVIS =
% patch([xdata(1,1:3)],[ydata(1,1:3)],[zdata(1,1:3)],ColorVec(1,:));

[RANK_X RANK_Y RANK_Z] = sphere(10); RIGHT_ANKLE = surf(RANK_X, RANK_Y, RANK_Z,'edgealpha',0,'FaceColor',ColorVec(2,:));
[LANK_X LANK_Y LANK_Z] = sphere(10); LEFT_ANKLE = surf(LANK_X, LANK_Y, LANK_Z,'edgealpha',0,'FaceColor',ColorVec(2,:));

[RKN_X RKN_Y RKN_Z] = sphere(10); RIGHT_KNEE = surf(RKN_X, RKN_Y, RKN_Z,'FaceColor',ColorVec(2,:),'edgealpha',0);
[LKN_X LKN_Y LKN_Z] = sphere(10); LEFT_KNEE = surf(LKN_X, LKN_Y, LKN_Z,'FaceColor',ColorVec(2,:),'edgealpha',0);

[RHP_X RHP_Y RHP_Z] = sphere(10); RIGHT_HIP = surf(RHP_X, RHP_Y, RHP_Z,'FaceColor',ColorVec(2,:),'edgealpha',0);
[LHP_X LHP_Y LHP_Z] = sphere(10); LEFT_HIP = surf(LHP_X, LHP_Y, LHP_Z,'FaceColor',ColorVec(2,:),'edgealpha',0);

[RLEG_X RLEG_Y RLEG_Z] = cylinder(leg_radius,10); RIGHT_LEG = surf(RLEG_X, RLEG_Y, RLEG_Z,'edgealpha',0,'FaceColor',ColorVec(1,:));
[LLEG_X LLEG_Y LLEG_Z] = cylinder(leg_radius,10); LEFT_LEG = surf(LLEG_X, LLEG_Y, LLEG_Z,'edgealpha',0,'FaceColor',ColorVec(1,:));

[RTHIGH_X RTHIGH_Y RTHIGH_Z] = cylinder(thigh_radius,10); RIGHT_THIGH = surf(RTHIGH_X, RTHIGH_Y, RTHIGH_Z,'edgealpha',0,'FaceColor',ColorVec(1,:));
[LTHIGH_X LTHIGH_Y LTHIGH_Z] = cylinder(thigh_radius,10); LEFT_THIGH = surf(LTHIGH_X, LTHIGH_Y, LTHIGH_Z,'edgealpha',0,'FaceColor',ColorVec(1,:));

[PELVIS_X PELVIS_Y PELVIS_Z] = pelvis_dim(1,.05); PELVIS_B = surf(PELVIS_X, PELVIS_Y, PELVIS_Z,'edgealpha',0,'FaceColor',ColorVec(1,:));
PELVIS_TOP = patch(PELVIS_X(1,:), PELVIS_Y(1,:), PELVIS_Z(1,:),ColorVec(1,:));

[RFOOT_X RFOOT_Y RFOOT_Z] = foot_dim('right',foot_scale,foot_height); RIGHT_FOOT = surf(RFOOT_X, RFOOT_Y, RFOOT_Z,'edgealpha',0,'FaceColor',ColorVec(1,:));
 RFOOT_TOP = patch([1 RFOOT_X(1,:)], [1 RFOOT_Y(1,:)], [1 RFOOT_Z(1,:)],ColorVec(1,:));
[LFOOT_X LFOOT_Y LFOOT_Z] = foot_dim('left',foot_scale,foot_height); LEFT_FOOT = surf(LFOOT_X, LFOOT_Y, LFOOT_Z,'edgealpha',0,'FaceColor',ColorVec(1,:));
LFOOT_TOP = patch(LFOOT_X(1,:), LFOOT_Y(1,:), LFOOT_Z(1,:),ColorVec(1,:));

% FORCE_VECTOR_L = line([CoP_L(1,1) CoP_L(1,1)+Lforce_dir(1,1)],[CoP_L(1,2) CoP_L(1,2)+Lforce_dir(1,2)], ...
%     [0 Lforce_dir(1,3)],'Color','r','LineWidth',3);
% FORCE_VECTOR_R = line([CoP_R(1,1) CoP_R(1,1)+Rforce_dir(1,1)],[CoP_R(1,2) CoP_R(1,2)+Rforce_dir(1,2)], ...
%     [0 Rforce_dir(1,3)],'Color','g','LineWidth',3);


RHeel = RHeel/1000;
for i =  1:RHS_locs(5)
%     set(PELVIS,'xdata',xdata(i,1:3),
%     'ydata',ydata(i,1:3),'zdata',zdata(i,1:3))
    if torque(10*i) > 1
        set(LEFT_THIGH,'FaceColor',[.1 1 1]);
        set(LEFT_LEG,'FaceColor',[.1 1 1]);
    else
        set(LEFT_THIGH,'FaceColor',ColorVec(1,:));
        set(LEFT_LEG,'FaceColor',ColorVec(1,:));
    end
%     set(FORCE_VECTOR_L,'xdata',[CoP_L(10*i,1) CoP_L(10*i,1)+Lforce_mag(10*i)*Lforce_dir(10*i,1)], ...
%         'ydata',[CoP_L(10*i,2) CoP_L(10*i,2)+Lforce_mag(10*i)*Lforce_dir(10*i,2)],...
%         'zdata',[0 Lforce_mag(10*i)*Lforce_dir(10*i,3)]);
%     set(FORCE_VECTOR_R,'xdata',[CoP_R(10*i,1) CoP_R(10*i,1)+Rforce_mag(10*i)*Rforce_dir(10*i,1)], ...
%         'ydata',[CoP_R(10*i,2) CoP_R(10*i,2)+Rforce_mag(10*i)*Rforce_dir(10*i,2)],...
%         'zdata',[0 Rforce_mag(10*i)*Rforce_dir(10*i,3)]);
%     if zdata(i,12) > min(zdata(:,12))+.015
%         set(FORCE_VECTOR_L,'Color','w','zdata',[0 0]);
%     else
%         set(FORCE_VECTOR_L,'Color','r');
%     end
% 
%     if zdata(i,20) > min(zdata(:,20))+.045
%         set(FORCE_VECTOR_R,'Color','w','zdata',[0 0]);
%     else
%         set(FORCE_VECTOR_R,'Color','g');
%     end
%% Right Thigh Coords
    RThigh_rot = coord_def([xdata(i,13) ydata(i,13) zdata(i,13)], [xdata(i,14) ydata(i,14) zdata(i,14)], [xdata(i,15) ydata(i,15) zdata(i,15)]);
    RKnee_CoR = RThigh_rot*RKnee_1';
    RKnee_CoR_med = RThigh_rot*RKnee_in_1';
    RHip_CoR = RThigh_rot*RHip_1';

%% Right Shank Coords
    RShank_rot = coord_def([xdata(i,16) ydata(i,16) zdata(i,16)],[xdata(i,17) ydata(i,17) zdata(i,17)],[xdata(i,18) ydata(i,18) zdata(i,18)]);
    RKnee_CoR_shank =  RShank_rot*RKnee_2';
    RKnee_CoR_shank_med = RShank_rot*RKnee_in_2';
    RKnee_low = 0.5*(RKnee_CoR_shank - RKnee_CoR_shank_med) + RKnee_CoR_shank_med;  % finding midpoint between medial and lateral knees
    RLeg_high = RShank_rot*[RLEG_X(2,:)+RKnee_low(1); RLEG_Y(2,:)+RKnee_low(2); RLEG_Z(2,:)-1+RKnee_low(3); ones(1,length(RLEG_Z(2,:)))];

%% Right Ankle Coords
    Rank_rot = coord_def([xdata(i,19) ydata(i,19) zdata(i,19)], [xdata(i,20) ydata(i,20) zdata(i,20)],[xdata(i,21) ydata(i,21) zdata(i,21)]);
    Rank_CoR = Rank_rot*Rank';

%% joint coordinates
    % Determining coordinates for right hip
    RHip = RHip_CoR - 0.5*(RKnee_CoR - RKnee_CoR_med);
    %  Determining coordinates for right knee
    Mid_1 = 0.5*(RKnee_CoR - RKnee_CoR_med) + RKnee_CoR_med;
    Mid_2 = 0.5*(RKnee_CoR_shank - RKnee_CoR_shank_med) + RKnee_CoR_shank_med;
    RKnee = 0.5*(Mid_1 - Mid_2) + Mid_2;
    % Determining coordinates for right ankle
    RAnkle = 0.5*([xdata(i,19) ydata(i,19) zdata(i,19) 1]' - Rank_CoR) + Rank_CoR;  % finding midpoint between medial and lateral ankles

%% body coordinate systems

    %% Determining coordinates for right shank
    Shank_origin = RAnkle;
    [matrix, x1,y1,z1] = coord_def(RAnkle(1:3)', RKnee_low(1:3)',[xdata(i,19) ydata(i,19) zdata(i,19)]); %rotation matrix for shank space
    [RLEG_X RLEG_Y RLEG_Z] = cylinder(leg_radius,10); %calculating points for cylinder
    matrix_y90 = rot_matrix('y',-pi/2);    % rotation matrix for cylinder since it only can be made in one direction
    cyl_low = matrix_y90*[RLEG_X(1,:);RLEG_Y(1,:);RLEG_Z(1,:);ones(1,length(RLEG_Z(1,:)))]; %rotating bottom cylinder
    cyl_high = matrix_y90*[RLEG_X(2,:);RLEG_Y(2,:);RLEG_Z(2,:)-1+norm(RKnee_low-RAnkle);ones(1,length(RLEG_Z(2,:)))]; %rotating top circle of cylinder
    RLeg_low = matrix*cyl_low;  %rotating cylinder in shank space
    RLeg_high = matrix*cyl_high;

    % Determining coordinates for right thigh
    Shank_origin = RKnee;
    [matrix, x1,y1,z1] = coord_def(RKnee(1:3)', RHip(1:3)',RHip_CoR(1:3)'); %rotation matrix for shank space
    [RTHIGH_X RTHIGH_Y RTHIGH_Z] = cylinder(thigh_radius,10); %calculating points for cylinder
    matrix_y90 = rot_matrix('y',-pi/2);    % rotation matrix for cylinder since it only can be made in one direction
    cyl_low = matrix_y90*[RTHIGH_X(1,:);RTHIGH_Y(1,:);RTHIGH_Z(1,:);ones(1,length(RTHIGH_Z(1,:)))]; %rotating bottom cylinder
    cyl_high = matrix_y90*[RTHIGH_X(2,:);RTHIGH_Y(2,:);RTHIGH_Z(2,:)-1+norm(RKnee-RHip);ones(1,length(RTHIGH_Z(2,:)))]; %rotating top circle of cylinder
    RThigh_low = matrix*cyl_low;  %rotating cylinder in shank space
    RThigh_high = matrix*cyl_high;

    
    % Determining coordinates for right foot
    [matrix, x1,y1,z1] = coord_def(RHeel(i,1:3), RToe(i,1:3)/1000,RAnkle(1:3)'); 
    [RFOOT_X RFOOT_Y RFOOT_Z] = foot_dim('right',foot_scale,foot_height); %calculating points for cylinder
    matrix_x = rot_matrix('x',pi/2+pi/9); matrix_y = rot_matrix('z',pi/2+pi/6); matrix_x = matrix_x*matrix_y;
    rf_low = matrix_x*[RFOOT_X(1,:);RFOOT_Y(1,:);RFOOT_Z(1,:);ones(1,length(RFOOT_Z(1,:)))]; %rotating bottom cylinder
    rf_high = matrix_x*[RFOOT_X(2,:);RFOOT_Y(2,:);RFOOT_Z(2,:);ones(1,length(RFOOT_Z(2,:)))]; %rotating top circle of cylinder
    RFoot_low = matrix*rf_low;
    RFoot_high = matrix*rf_high;
    
%     line([RHeel(1) x1(1)+RHeel(1)],[RHeel(2) x1(2)+RHeel(2)],[RHeel(3) x1(3)+RHeel(3)],'Color','r')
%     line([RHeel(1) y1(1)+RHeel(1)],[RHeel(2) y1(2)+RHeel(2)],[RHeel(3) y1(3)+RHeel(3)],'Color','g')
%     line([RHeel(1) z1(1)+RHeel(1)],[RHeel(2) z1(2)+RHeel(2)],[RHeel(3) z1(3)+RHeel(3)],'Color','k')


%% Plotting Right leg
    set(RIGHT_LEG,'xdata',[RLeg_high(1,:); RLeg_low(1,:)],'ydata',[RLeg_high(2,:); RLeg_low(2,:)],'zdata',[RLeg_high(3,:); RLeg_low(3,:)]);
    set(RIGHT_THIGH,'xdata',[RThigh_high(1,:); RThigh_low(1,:)],'ydata',[RThigh_high(2,:); RThigh_low(2,:)],'zdata',[RThigh_high(3,:); RThigh_low(3,:)]);
    set(RIGHT_ANKLE,'xdata', [ankle_size*RANK_X + RAnkle(1)], 'ydata', [ankle_size*RANK_Y + RAnkle(2)], ...
        'zdata', [ankle_size*RANK_Z + RAnkle(3)]);
    set(RIGHT_KNEE,'xdata', [knee_size*RKN_X + RKnee(1)], 'ydata', [knee_size*RKN_Y + RKnee(2)], ...
        'zdata', [knee_size*RKN_Z + RKnee(3)]);
    set(RIGHT_HIP,'xdata', [hip_size*LHP_X + RHip(1)], 'ydata', [hip_size*RHP_Y + RHip(2)], ...
        'zdata', [hip_size*RHP_Z + RHip(3)]);

    set(RIGHT_FOOT,'xdata',[RFoot_high(1,:); RFoot_low(1,:)], 'ydata',[RFoot_high(2,:); RFoot_low(2,:)],'zdata',[RFoot_high(3,:); RFoot_low(3,:)]);
    set(RFOOT_TOP,'xdata',[RAnkle(1) RFoot_low(1,:)],'ydata',[RAnkle(2) RFoot_low(2,:)],'zdata',[RAnkle(3) RFoot_low(3,:)]);
%% Pelvic Coordinate System

    [matrix, x1,y1,z1] = coord_def([xdata(i,3), ydata(i,3), zdata(i,3)],[xdata(i,1), ydata(i,1), zdata(i,1)],[xdata(i,2), ydata(i,2), zdata(i,2)]); 
    [PELVIS_X PELVIS_Y PELVIS_Z] = pelvis_dim(3,pelvis_height); %calculating points for cylinder
    matrix_z = rot_matrix('z',-2*pi/3-pi/50);
    p_low = matrix_z*[PELVIS_X(1,:);PELVIS_Y(1,:);PELVIS_Z(1,:);ones(1,length(PELVIS_Z(1,:)))]; %rotating bottom cylinder
    p_high = matrix_z*[PELVIS_X(2,:);PELVIS_Y(2,:);PELVIS_Z(2,:);ones(1,length(PELVIS_Z(2,:)))]; %rotating top circle of cylinder
    Pelvis_low = matrix*p_low;
    Pelvis_high = matrix*p_high;
    %plotting
    set(PELVIS_B,'xdata',[Pelvis_high(1,:); Pelvis_low(1,:)],'ydata',[Pelvis_high(2,:); Pelvis_low(2,:)],'zdata',[Pelvis_high(3,:); Pelvis_low(3,:)]);
    set(PELVIS_TOP,'xdata',Pelvis_low(1,:),'ydata',Pelvis_low(2,:),'zdata',Pelvis_low(3,:));
%% Calculating left femoral coords
    [LThigh_rot, x1,y1,z1] = coord_def([xdata(i,4) ydata(i,4) zdata(i,4)], [xdata(i,5) ydata(i,5) zdata(i,5)],[xdata(i,6) ydata(i,6) zdata(i,6)]); 
    LKnee_CoR = LThigh_rot*LKnee_1';
    LKnee_CoR_med = LThigh_rot*LKnee_in_1';
    LHip_CoR = LThigh_rot*LHip_1';

%% Calculating Left shank coords
   [LShank_rot, x1,y1,z1] = coord_def([xdata(i,7) ydata(i,7) zdata(i,7)], [xdata(i,8) ydata(i,8) zdata(i,8)],[xdata(i,9) ydata(i,9) zdata(i,9)]); 
    LKnee_CoR_shank = LShank_rot*LKnee_2';
    LKnee_CoR_shank_med = LShank_rot*LKnee_in_2';
    LKnee_low = 0.5*(LKnee_CoR_shank - LKnee_CoR_shank_med) + LKnee_CoR_shank_med;  % finding midpoint between medial and lateral knees
    LLeg_high = LShank_rot*[LLEG_X(2,:)+LKnee_low(1); LLEG_Y(2,:)+LKnee_low(2); LLEG_Z(2,:)-1+LKnee_low(3); ones(1,length(LLEG_Z(2,:)))];
    
%% Left Ankle Coords
    Lank_rot = coord_def([xdata(i,10) ydata(i,10) zdata(i,10)], [xdata(i,11) ydata(i,11) zdata(i,11)],[xdata(i,12) ydata(i,12) zdata(i,12)]);
    Lank_CoR = Lank_rot*Lank';

%% joint coordinates
    % Determining coordinates for right hip
    LHip = LHip_CoR - 0.5*(LKnee_CoR - LKnee_CoR_med);
    %  Determining coordinates for right knee
    Mid_1 = 0.5*(LKnee_CoR - LKnee_CoR_med) + LKnee_CoR_med;
    Mid_2 = 0.5*(LKnee_CoR_shank - LKnee_CoR_shank_med) + LKnee_CoR_shank_med;
    LKnee = 0.5*(Mid_1 - Mid_2) + Mid_2;
    % Determining coordinates for right ankle
    LAnkle = 0.5*([xdata(i,10) ydata(i,10) zdata(i,10) 1]' - Lank_CoR) + Lank_CoR;  % finding midpoint between medial and lateral ankles

%% body coordinate systems

    %% Determining coordinates for right shank
    %Shank_origin = LAnkle;
    [matrix, x1,y1,z1] = coord_def(LAnkle(1:3)', LKnee_low(1:3)',[xdata(i,10) ydata(i,10) zdata(i,10)]); %rotation matrix for shank space
    [LLEG_X LLEG_Y LLEG_Z] = cylinder(leg_radius,10); %calculating points for cylinder
    matrix_y90 = rot_matrix('y',-pi/2);    % rotation matrix for cylinder since it only can be made in one direction
    cyl_low = matrix_y90*[LLEG_X(1,:);LLEG_Y(1,:);LLEG_Z(1,:);ones(1,length(RLEG_Z(1,:)))]; %rotating bottom cylinder
    cyl_high = matrix_y90*[LLEG_X(2,:);LLEG_Y(2,:);LLEG_Z(2,:)-1+norm(LKnee_low-LAnkle);ones(1,length(LLEG_Z(2,:)))]; %rotating top circle of cylinder
    LLeg_low = matrix*cyl_low;  %rotating cylinder in shank space
    LLeg_high = matrix*cyl_high;


  % Determining coordinates for right thigh
    %Shank_origin = RKnee;
    [matrix, x1,y1,z1] = coord_def(LKnee(1:3)', LHip(1:3)',LHip_CoR(1:3)'); %rotation matrix for shank space
    [LTHIGH_X LTHIGH_Y LTHIGH_Z] = cylinder(thigh_radius,10); %calculating points for cylinder
    matrix_y90 = rot_matrix('y',-pi/2);    % rotation matrix for cylinder since it only can be made in one direction
    cyl_low = matrix_y90*[LTHIGH_X(1,:);LTHIGH_Y(1,:);LTHIGH_Z(1,:);ones(1,length(LTHIGH_Z(1,:)))]; %rotating bottom cylinder
    cyl_high = matrix_y90*[LTHIGH_X(2,:);LTHIGH_Y(2,:);LTHIGH_Z(2,:)-1+norm(LKnee-LHip);ones(1,length(LTHIGH_Z(2,:)))]; %rotating top circle of cylinder
    LThigh_low = matrix*cyl_low;  %rotating cylinder in shank space
    LThigh_high = matrix*cyl_high;
    
    % determining left foot coords
    [matrix, x1,y1,z1] = coord_def(LHeel(i,1:3)/1000, LToe(i,1:3)/1000,LAnkle(1:3)'); 
    [LFOOT_X LFOOT_Y LFOOT_Z] = foot_dim('left',foot_scale,foot_height); %calculating points for cylinder
    matrix_x = rot_matrix('x',pi/3); matrix_y = rot_matrix('z',pi/3); matrix_x = matrix_x*matrix_y;
    lf_low = matrix_x*[LFOOT_X(1,:);LFOOT_Y(1,:);LFOOT_Z(1,:);ones(1,length(LFOOT_Z(1,:)))]; %rotating bottom cylinder
    lf_high = matrix_x*[LFOOT_X(2,:);LFOOT_Y(2,:);LFOOT_Z(2,:);ones(1,length(LFOOT_Z(2,:)))]; %rotating top circle of cylinder
    LFoot_low = matrix*lf_low;
    LFoot_high = matrix*lf_high;
    
%% Kinematics
%Knee Flexion Angle 
% LKnee_Flexion(i) = atan2(LHip(3) - LKnee(3),LHip(1) - LKnee(1)) - atan2(LKnee(3) - LAnkle(3),LKnee(1) - LAnkle(1));
% LKnee_Flexion_vel(i) = diff(LKnee_Flexion)/dt_trc;
% RKnee_Flexion = atan2(RHip(3) - RKnee(3),RHip(1) - RKnee(1)) - atan2(RKnee(3) - RAnkle(3),RKnee(1) - RAnkle(1));
% RKnee_Flexion_vel = diff(RKnee_Flexion)/dt_trc;
% [b,a] = butter(5,MA_filter/50); %100 Hz sampling rate
% LKnee_Flexion_vel = filtfilt(b,a,LKnee_Flexion_vel); RKnee_Flexion_vel = filtfilt(b,a,RKnee_Flexion_vel);
    
%% plotting left leg
    set(LEFT_LEG,'xdata',[LLeg_high(1,:); LLeg_low(1,:)],'ydata',[LLeg_high(2,:); LLeg_low(2,:)],'zdata',[LLeg_high(3,:); LLeg_low(3,:)]);
    set(LEFT_THIGH,'xdata',[LThigh_high(1,:); LThigh_low(1,:)],'ydata',[LThigh_high(2,:); LThigh_low(2,:)],'zdata',[LThigh_high(3,:); LThigh_low(3,:)]);
    
     set(LEFT_ANKLE,'xdata', ankle_size*LANK_X + LAnkle(1), 'ydata', ankle_size*LANK_Y + LAnkle(2), ...
        'zdata', ankle_size*LANK_Z + LAnkle(3));
    set(LEFT_KNEE,'xdata', [knee_size*LKN_X + LKnee(1)], 'ydata', [knee_size*LKN_Y + LKnee(2)], ...
        'zdata', [knee_size*LKN_Z + LKnee(3)]);
    set(LEFT_HIP,'xdata', [hip_size*LHP_X + LHip(1)], 'ydata', [hip_size*LHP_Y + LHip(2)], ...
        'zdata', [hip_size*LHP_Z + LHip(3)]);
    
    set(LEFT_FOOT,'xdata',[LFoot_high(1,:); LFoot_low(1,:)], 'ydata',[LFoot_high(2,:); LFoot_low(2,:)],'zdata',[LFoot_high(3,:); LFoot_low(3,:)]);
    set(LFOOT_TOP,'xdata',[LAnkle(1) LFoot_low(1,:)],'ydata',[LAnkle(2) LFoot_low(2,:)],'zdata',[LAnkle(3) LFoot_low(3,:)]);
    
%%  Plotting spatiotemporal data
    if i > LTO_locs(2) && i < LHS_locs(3)
        LANKLE_TRACE(i) = plot3(xdata(i,11), ydata(i,11), zdata(i,11),'.r');
    elseif i == LHS_locs(3)+1
        LSTEP_LENGTH = line([xdata(LTO_locs(2),11) xdata(LHS_locs(2),11)],[ydata(LTO_locs(2),11) ydata(LHS_locs(2),11)], [0.01 0.01],'Color','r','LineWidth',3);
    elseif i == LHS_locs(4)
        erase_ind = find(LANKLE_TRACE); delete(LANKLE_TRACE(erase_ind));
    elseif i > LTO_locs(3)-20 && i < LHS_locs(4)
           LPELVIS_TRACE(i) = line([LHip(1) xdata(i,1)],[LHip(2) ydata(i,1)],[LHip(3) zdata(i,1)],'Color',hsv2rgb([1 1 (lknee_vel(i)/max(lknee_vel))/3+0.5]));
        LTHIGH_TRACE(i) = line([LKnee(1) LHip(1)],[LKnee(2) LHip(2)],[LKnee(3) LHip(3)],'Color',hsv2rgb([1 1 (lknee_vel(i)/max(lknee_vel))/3+0.5]));
        LLEG_TRACE(i) = line([LKnee(1) LAnkle(1)],[LKnee(2) LAnkle(2)],[LKnee(3) LAnkle(3)],'Color',hsv2rgb([1 1 (lknee_vel(i)/max(lknee_vel))/3+0.5]));
        LFOOT_TRACE_HIGH(i) = line([LAnkle(1) xdata(i,11)], [LAnkle(2) ydata(i,11)], [LAnkle(3) zdata(i,11)],'Color','r');
        LFOOT_TRACE(i) = line([xdata(i,11) l_midmet(i,1)],[ydata(i,11) l_midmet(i,2)],[zdata(i,11) l_midmet(i,3)],'Color','r');
    elseif i == LHS_locs(4)+1
        LFOOT_CLEARANCE = line([l_midmet(Max_LToe_ind,1) l_midmet(Max_LToe_ind,1)], [l_midmet(Max_LToe_ind,2) l_midmet(Max_LToe_ind,2)], ...
            [0 l_midmet(Max_LToe_ind,3)],'Color','r','LineWidth',3);
    elseif i == LHS_locs(4)
        erase_ind = find(LFOOT_TRACE); delete(LFOOT_TRACE(erase_ind));
        erase_ind = find(LFOOT_TRACE_HIGH); delete(LFOOT_TRACE_HIGH(erase_ind));
    end
    if i > RTO_locs(2) && i < RHS_locs(3)
        RANKLE_TRACE(i) = plot3(xdata(i,20), ydata(i,20), zdata(i,20),'.g');
    elseif i == RHS_locs(3)+1   
        RSTEP_LENGTH = line([xdata(RTO_locs(2),20) xdata(RHS_locs(3),20)],[ydata(RTO_locs(2),20) ydata(RHS_locs(3),20)], [0.04 0.04],'Color','g','LineWidth',3);
    elseif i == LHS_locs(4)
         erase_ind = find(RANKLE_TRACE); delete(RANKLE_TRACE(erase_ind));
    elseif i > RTO_locs(3)-20 && i < RHS_locs(4)
        RPELVIS_TRACE(i) = line([RHip(1) xdata(i,2)],[RHip(2) ydata(i,2)],[RHip(3) zdata(i,2)],'Color',hsv2rgb([0.3 1 (rknee_vel(i)/max(rknee_vel))/3+0.5]));
        RTHIGH_TRACE(i) = line([RKnee(1) RHip(1)],[RKnee(2) RHip(2)],[RKnee(3) RHip(3)],'Color',hsv2rgb([0.3 1 (rknee_vel(i)/max(rknee_vel))/3+0.5]));
        RLEG_TRACE(i) = line([RKnee(1) RAnkle(1)],[RKnee(2) RAnkle(2)],[RKnee(3) RAnkle(3)],'Color',hsv2rgb([0.3 1 (rknee_vel(i)/max(rknee_vel))/3+0.5]));
        RFOOT_TRACE_HIGH(i) = line([RAnkle(1) xdata(i,20)], [RAnkle(2) ydata(i,20)], [RAnkle(3) zdata(i,20)],'Color','g');
        RFOOT_TRACE(i) = line([xdata(i,20) r_midmet(i,1)],[ydata(i,20) r_midmet(i,2)],[zdata(i,20) r_midmet(i,3)],'Color','g');
    elseif i == RHS_locs(4)+1
        RFOOT_CLEARANCE = line([r_midmet(Max_RToe_ind,1) r_midmet(Max_RToe_ind,1)], [r_midmet(Max_RToe_ind,2) r_midmet(Max_RToe_ind,2)], ...
            [0 r_midmet(Max_RToe_ind,3)],'Color','g','LineWidth',3);
    elseif i == RHS_locs(5)
        erase_ind = find(RFOOT_TRACE); delete(RFOOT_TRACE(erase_ind));
        erase_ind = find(RFOOT_TRACE_HIGH); delete(RFOOT_TRACE_HIGH(erase_ind));
     end


drawnow;
pause(dt_trc);

end
delete(LEFT_LEG); delete(RIGHT_LEG); delete(LEFT_THIGH); delete(RIGHT_THIGH); delete(LEFT_FOOT); delete(RIGHT_FOOT); delete(LFOOT_TOP); delete(RFOOT_TOP);
delete(LEFT_ANKLE); delete(RIGHT_ANKLE); delete(PELVIS_B); delete(PELVIS_TOP); delete(LEFT_HIP); delete(RIGHT_HIP); delete(LEFT_KNEE); delete(RIGHT_KNEE);
% leg_ind = find(RTHIGH_TRACE); set(RTHIGH_TRACE(leg_ind,ones(1,length(leg_ind))),'LineWidth',2); %set(RLEG_TRACE(leg_ind,1),'LineWidth',2); set(RFOOT_TRACE_HIGH(leg_ind),'LineWidth',2); set(RFOOT_TRACE(leg_ind),'LineWidth',2);



